#pragma once
#include <vector>

#include "gtest/gtest.h"
#include "Utility/FileSystem/SGFileSystem.h"
#include "DataStructure/Common/Constant.h"

#include "DBManager/IService/Input/Element/IElementService.h"
#include "DBManager/IService/Input/Load/IFORCEService.h"

#include "Import/ImportAPI/ImportAPI.h"
#include "TestData/model/Link_TwoELe.hpp"
#include "TestData/model/OneBeam.hpp"
#include "TestData/model/StraightBeam_LinkByRBAR1.hpp"
#include "TestData/model/StraightBeam_LinkByRBE2.hpp"

#include "Utility/Algebra/sgEigenSpace.h"

#include "../Assembly.h"

using SG::Algebra::Matrix;
using SG::DataStructure::Common::Status;
using SG::DataStructure::Common::Id;
using SG::DataStructure::Common::TOLERANCE;
using SG::DataStructure::FEM::CONRODData;
using SG::DataStructure::FEM::CRODData;
using SG::DataStructure::FEM::DOFDirections;
using SG::DataStructure::FEM::DofSet;
using SG::DataStructure::FEM::FORCEData;
using SG::Algebra::convertTripletsToMKLSparse;

using SG::FEMSolver::AssemblyEigen;

extern std::shared_ptr<DBServiceFactory> DB_SERVICEFACTORY;
SG_NAMESPACE_OPEN
namespace FEMSolver
{
    bool Equal (Eigen::SparseMatrix<double>& sparse, const SG::Algebra::Matrix& dense)
    {
        std::size_t nRow = dense.GetRowSize ();
        std::size_t nCol = dense.GetColSize ();

        int                 row  = 0;
        int                 col  = 0;
        double              valu = 0.0;
        SG::Algebra::Matrix matr (nRow, nCol);
        for (int k = 0; k < sparse.outerSize (); ++k)
        {
            for (Eigen::SparseMatrix<double>::InnerIterator it (sparse, k); it; ++it)
            {
                row            = it.row ();    // row index
                col            = it.col ();    // col index
                valu           = it.value ();  // val index
                matr[row][col] = valu;
            }
        }

        if (dense == matr)
        {
            return true;
        }
        else
        {
            return false;
        }
    }

    TEST (AssemblyEigenTest, MatrixchunksCace)
    {
        // 稀疏矩阵
        //  0	 3	 0	 0	 0
        //  22	 0	 0	 0	 17
        //  7	 5	 0	 1	 0
        //  0	 0	 0	 0	 0
        //  0	 0	 14	 0	 8

        // Input
        int                                 m      = 5;
        int                                 n      = 8;
        int                                 row[n] = { 0, 1, 1, 2, 2, 2, 4, 4 };     // 非零元素的行号
        int                                 col[n] = { 1, 0, 4, 0, 1, 3, 2, 4 };     // 非零元素的列号
        double                              val[n] = { 3, 22, 17, 7, 5, 1, 14, 8 };  // 非零元素的值
        Eigen::SparseMatrix<double>         Evmatr (m, m);
        std::vector<Eigen::Triplet<double>> triplet_list;
        for (int i = 0; i < n; ++i)
        {
            triplet_list.emplace_back (Eigen::Triplet<double> (row[i], col[i], val[i]));
        }

        // Evmatr.setFromTriplets (triplet_list.begin (), triplet_list.end ());
        convertTripletsToMKLSparse (triplet_list.begin (), triplet_list.end (), Evmatr);
        // 矩阵分块输入参数
        SG::DataStructure::Common::Id split = 2;
        Eigen::SparseMatrix<double>   mat1;
        Eigen::SparseMatrix<double>   mat2;
        Eigen::SparseMatrix<double>   mat3;
        Eigen::SparseMatrix<double>   mat4;
        AssemblyEigen::Matrixchunks (Evmatr, split, _OUT mat1, _OUT mat2, _OUT mat3, _OUT mat4);

        // Target data
        SG::Algebra::Matrix mat1Target (2, 2, { 0.0, 3.0, 22.0, 0.0 });
        SG::Algebra::Matrix mat2Target (2, 3, { 0.0, 0.0, 0.0, 0.0, 0.0, 17.0 });
        SG::Algebra::Matrix mat3Target (3, 2, { 7.0, 5.0, 0.0, 0.0, 0.0, 0.0 });
        SG::Algebra::Matrix mat4Target (3, 3, { 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 14.0, 0.0, 8.0 });

        // Check
        ASSERT_TRUE (Equal (mat1, mat1Target));
        ASSERT_TRUE (Equal (mat2, mat2Target));
        ASSERT_TRUE (Equal (mat3, mat3Target));
        ASSERT_TRUE (Equal (mat4, mat4Target));
    }

    TEST (AssemblyRBAR1ElementTest, AccumulateControlMatrix)
    {
        // Input data
        StraightBeam_LinkByRBAR1 model;
        auto                     dbServiceFactory = model.GetModel ();
        auto&                    assembly         = AssemblyEigen::GetInstance ();
        assembly.Clear ();

        auto para = dbServiceFactory->get<ISubcaseService> ()->FindSubCase (1);
        SG::DataStructure::FEM::Subcase subcase (para);
        assembly.Initialize (dbServiceFactory, subcase, { 1 });

        // 等价模型中RBAR1计算得到的数据
        std::vector<int> gdofn{ 0, 1, 2, 3, 4, 5, 18, 19, 20, 21, 22, 23 };
        std::vector<int> gdofm{ 18, 19, 20, 21, 22, 23 };

        SG::Algebra::Matrix RnRm (6, 12);
        RnRm.BlockFill (0, 0, SG::Algebra::identity (6));
        RnRm (1, 5) = -0.5;
        RnRm (2, 4) = 0.5;
        RnRm.BlockFill (0, 6, SG::Algebra::identity (6) * -1.0);

        assembly.AccumulateControlMatrix (gdofn, gdofm, RnRm);
        assembly.Assembly (dbServiceFactory);
        assembly.Constraint (dbServiceFactory, 1);

        // Test data
        auto GmTest = assembly.GetControlMatrix ();

        // Check
        ASSERT_EQ (6, GmTest.rows ());
        ASSERT_EQ (18, GmTest.cols ());
        for (std::size_t index = 0; index < 6; ++index)
        {
            ASSERT_NEAR (GmTest.coeffRef (index, index), 1.0, TOLERANCE);
        }

        ASSERT_NEAR (GmTest.coeffRef (1, 5), -0.5, TOLERANCE);
        ASSERT_NEAR (GmTest.coeffRef (2, 4), 0.5, TOLERANCE);
    }
};  // end namespace FEMSolver
SG_NAMESPACE_CLOSE

class AssemblyTwoTrussCaseTest : public ::testing::Test
{
  public:
    static void SetUpTestSuite ()
    {
        // Import
        Link_TwoEle model;
        dbServiceFactory = model.GetModel ();

        // Initialize
        auto& assembly = AssemblyEigen::GetInstance ();
        assembly.Clear ();

        auto para = dbServiceFactory->get<ISubcaseService> ()->FindSubCase (1);
        SG::DataStructure::FEM::Subcase subcase (para);
        assembly.Initialize (dbServiceFactory, subcase, { 1 });
    }
    static SG::DBManager::DBServiceFactorySP dbServiceFactory;
};

DBServiceFactorySP AssemblyTwoTrussCaseTest::dbServiceFactory;

TEST_F (AssemblyTwoTrussCaseTest, FindNodeIndex)
{
    // Input
    CRODData link;
    // dbServiceFactory -> GetLinkAt (ElementTypes::ELETYPE_Link, 0, link);
    dbServiceFactory->get<IElementService> ()->FindByIndex (0, link);
    // Test Data
    auto&                    assembly = AssemblyEigen::GetInstance ();
    std::vector<std::size_t> nodeIndexesTest (2);
    assembly.FindNodeIndex (link.m_g, nodeIndexesTest);

    // Target data
    std::vector<std::size_t> nodeIndexesTarget{ 0, 2 };

    // Check
    ASSERT_TRUE (nodeIndexesTest == nodeIndexesTarget);
};

TEST_F (AssemblyTwoTrussCaseTest, AccumulateStiffnessTest)
{
    // Input
    auto& assembly = AssemblyEigen::GetInstance ();

    // 假设单元计算得到单刚
    DofSet              DOFs{ DOFDirections::DOF_Ux, DOFDirections::DOF_Uy, DOFDirections::DOF_Uz };
    SG::Algebra::Matrix stiffness1 (6, 6);
    stiffness1 (3, 3) = 1.0;
    stiffness1 (3, 4) = 2.0;
    stiffness1 (4, 3) = 3.0;
    stiffness1 (4, 4) = 1.0;

    SG::Algebra::Matrix stiffness2 (6, 6);
    stiffness2 (3, 3) = 1.0;
    stiffness2 (3, 4) = 2.0;
    stiffness2 (4, 3) = 3.0;
    stiffness2 (4, 4) = 1.0;

    std::vector<SG::DataStructure::Common::Id> ele1NodeId{ 1, 3 };
    std::vector<SG::DataStructure::Common::Id> ele2NodeId{ 2, 3 };

    std::vector<int> dofGlobalIndex;

    // Test data 模拟单元计算将刚度矩阵传入 Assembly 模块
    assembly.GetGlobalDOFId ({ 0, 2 }, DOFs, _OUT dofGlobalIndex);
    assembly.AccumulateStiffness (dofGlobalIndex, stiffness1);

    assembly.GetGlobalDOFId ({ 1, 2 }, DOFs, _OUT dofGlobalIndex);
    assembly.AccumulateStiffness (dofGlobalIndex, stiffness2);

    assembly.Assembly (dbServiceFactory);
    const auto& K = assembly.GetStiffnessMatrix ();

    // Check
    ASSERT_TRUE (2.0 == K.coeff (0, 0));
    ASSERT_TRUE (4.0 == K.coeff (0, 1));
    ASSERT_TRUE (6.0 == K.coeff (1, 0));
    ASSERT_TRUE (2.0 == K.coeff (1, 1));
}

TEST_F (AssemblyTwoTrussCaseTest, ComputeForceTest)
{
    // Input
    auto& assembly = AssemblyEigen::GetInstance ();

    // Test 模拟计算集中力
    FORCEDataArray forsetSet;
    dbServiceFactory->get<IFORCEService> ()->FindBySId (1, forsetSet);
    auto force = forsetSet[0];
    const auto n = force.GetLoad ();
    Matrix forceVector (3, 1, {n.m_x * force.m_factor, n.m_y * force.m_factor, n.m_z * force.m_factor});
    assembly.AccumulateLoad (1, { 0, 1, 2 }, forceVector);
    // 先计算控制矩阵
    assembly.Assembly (dbServiceFactory);
    assembly.Constraint (dbServiceFactory, 1);

    SG::FEMSolver::EigenVector fTest;
    Id                         sid = 1;
    assembly.ComputeBVector (sid, fTest);

    // Target data
    SG::FEMSolver::EigenVector fTarget (2);
    fTarget.setZero (2);
    fTarget (0) = 1.5;
    fTarget (1) = 3.0;

    // Check
    ASSERT_TRUE (fTarget == fTest);
}

class AssemblyOneBeamCantileverCaseTest : public ::testing::Test
{
  public:
    static void SetUpTestSuite ()
    {
        OneBeamCantilever model;

        dbServiceFactory = model.GetModel ();
        // 初始化
        auto& assembly = AssemblyEigen::GetInstance ();
        assembly.Clear ();
        auto para = dbServiceFactory->get<ISubcaseService> ()->FindSubCase (1);
        SG::DataStructure::FEM::Subcase subcase (para);
        assembly.Initialize (dbServiceFactory, subcase, { 1 });
    }
    static SG::DBManager::DBServiceFactorySP dbServiceFactory;
};

DBServiceFactorySP AssemblyOneBeamCantileverCaseTest::dbServiceFactory;

TEST_F (AssemblyOneBeamCantileverCaseTest, ComputeMomentTest)
{
    // Input
    auto&          assembly = AssemblyEigen::GetInstance ();
    FORCEDataArray forsetSet;
    dbServiceFactory->get<IFORCEService> ()->FindBySId (1, forsetSet);

    auto force = forsetSet[0];
    auto n = force.GetLoad ();
    Matrix forceVector (3, 1, {n.m_x * force.m_factor, n.m_y * force.m_factor, n.m_z * force.m_factor});
    assembly.AccumulateLoad (1, { 0, 1, 2 }, forceVector);

    FORCEDataArray moments;
    const Id       sid = 1;
    dbServiceFactory->get<IMOMENTService> ()->FindBySId (sid, moments);

    // Test 模拟组装弯矩
    auto moment = moments[0];
    auto n2 = moment.GetLoad ();
    Matrix momVector (3, 1, {n2.m_x * moment.m_factor, n2.m_y * moment.m_factor, n2.m_z * moment.m_factor});
    assembly.AccumulateLoad (sid, { 3, 4, 5 }, momVector);

    // assembly.Assembly (dbServiceFactory, 1);
    assembly.Assembly (dbServiceFactory);
    assembly.Constraint (dbServiceFactory, 1);

    SG::FEMSolver::EigenVector fTest;
    assembly.ComputeBVector (sid, _OUT fTest);

    // Check
    ASSERT_NEAR (320.0, fTest (2), TOLERANCE);
    ASSERT_NEAR (290.0, fTest (4), TOLERANCE);
}

TEST (AssemblyTest, CWELDDOFSortTest)
{
    // Import
    DB_SERVICEFACTORY->ClearDatabase ();
    SG::Import::SGImporter importer (DB_SERVICEFACTORY);
    auto fileName = SG::Utility::FileSystem::getTestDataPath ("SGFem/Pre/") + "Cweld_ELEMID_S2SSortTest.bdf";

    auto status = importer.Import (fileName);
    ASSERT_EQ (status, Status::STATUS_Success);

    // Test Data (查询虚节点的全局自由度编号)
    auto& assembly = AssemblyEigen::GetInstance();
    assembly.Clear ();
    SG::DataStructure::FEM::Subcase subcase;
    subcase.AddParameter({"SPC","100"});
    assembly.Initialize (DB_SERVICEFACTORY, subcase, {1});

    // Target
    std::vector<int> globalDofId_14 = {-1, -1, -1, -1, -1, -1};
    std::vector<int> globalDofId_GA = {78, 79, 80, 81, 82, 83};

    // Check
    static const DofSet alldofSet = SG::DataStructure::FEM::getAllNodeDofs ();
    std::vector<int> globalDofId_14Test;
    assembly.ComputeGlobalDOFId (14, alldofSet, globalDofId_14Test);
    ASSERT_TRUE (globalDofId_14Test == globalDofId_14);

    std::vector<int> globalDofId_GATest;
    assembly.ComputeGlobalDOFId (101000001, alldofSet, globalDofId_GATest);
    ASSERT_TRUE (globalDofId_GATest == globalDofId_GA);
}